/**************************************************************************************************
* Title: SumMatrix_16f.cpp
* Author: Gael Huber
* Description: Implements 16-byte aligned matrix functions
**************************************************************************************************/
#include "SumMath.h"

/**************************************************************************************************
* Multiply the matrix by another matrix and returns the result
**************************************************************************************************/
Matrix_16f& Matrix_16f::operator*=(const Matrix_16f& rhs) {
	// R0
	// Single vectors representing a particular row-column value
	// Create the resultant row values by multiplying across the matrix columns
	// (a*a, a*b, a*c, a*d), (b*e, b*f, b*g, b*h), ...
	// These will then be summed across to yield the row values
	__m128 v0(_mm_mul_ps(_mm_load1_ps(&_11), rhs.v[0]));
	__m128 v1(_mm_mul_ps(_mm_load1_ps(&_12), rhs.v[1]));
	__m128 v2(_mm_mul_ps(_mm_load1_ps(&_13), rhs.v[2]));
	__m128 v3(_mm_mul_ps(_mm_load1_ps(&_14), rhs.v[3]));

	// Sum respective column values
	v[0] = _mm_add_ps(v0, _mm_add_ps(v1, _mm_add_ps(v2, v3)));

	// R1
	v0 = _mm_mul_ps(_mm_load1_ps(&_21), rhs.v[0]);
	v1 = _mm_mul_ps(_mm_load1_ps(&_22), rhs.v[1]);
	v2 = _mm_mul_ps(_mm_load1_ps(&_23), rhs.v[2]);
	v3 = _mm_mul_ps(_mm_load1_ps(&_24), rhs.v[3]);
	v[1] = _mm_add_ps(v0, _mm_add_ps(v1, _mm_add_ps(v2, v3)));

	// R2
	v0 = _mm_mul_ps(_mm_load1_ps(&_31), rhs.v[0]);
	v1 = _mm_mul_ps(_mm_load1_ps(&_32), rhs.v[1]);
	v2 = _mm_mul_ps(_mm_load1_ps(&_33), rhs.v[2]);
	v3 = _mm_mul_ps(_mm_load1_ps(&_34), rhs.v[3]);
	v[2] = _mm_add_ps(v0, _mm_add_ps(v1, _mm_add_ps(v2, v3)));

	// R3
	v0 = _mm_mul_ps(_mm_load1_ps(&_41), rhs.v[0]);
	v1 = _mm_mul_ps(_mm_load1_ps(&_42), rhs.v[1]);
	v2 = _mm_mul_ps(_mm_load1_ps(&_43), rhs.v[2]);
	v3 = _mm_mul_ps(_mm_load1_ps(&_44), rhs.v[3]);
	v[3] = _mm_add_ps(v0, _mm_add_ps(v1, _mm_add_ps(v2, v3)));

	return *this;
}

/**************************************************************************************************
* Multiplies two matrices and returns the result
**************************************************************************************************/
Matrix_16f Matrix_16f::operator*(const Matrix_16f& rhs) const {
	// R0
	// Single vectors representing a particular row-column value
	// Create the resultant row values by multiplying across the matrix columns
	// (a*a, a*b, a*c, a*d), (b*e, b*f, b*g, b*h), ...
	// These will then be summed across to yield the row values
	__m128 v0(_mm_mul_ps(_mm_load1_ps(&_11), rhs.v[0]));
	__m128 v1(_mm_mul_ps(_mm_load1_ps(&_12), rhs.v[1]));
	__m128 v2(_mm_mul_ps(_mm_load1_ps(&_13), rhs.v[2]));
	__m128 v3(_mm_mul_ps(_mm_load1_ps(&_14), rhs.v[3]));

	// Sum respective column values
	__m128 r0(_mm_add_ps(v0, _mm_add_ps(v1, _mm_add_ps(v2, v3))));

	// R1
	v0 = _mm_mul_ps(_mm_load1_ps(&_21), rhs.v[0]);
	v1 = _mm_mul_ps(_mm_load1_ps(&_22), rhs.v[1]);
	v2 = _mm_mul_ps(_mm_load1_ps(&_23), rhs.v[2]);
	v3 = _mm_mul_ps(_mm_load1_ps(&_24), rhs.v[3]);
	__m128 r1(_mm_add_ps(v0, _mm_add_ps(v1, _mm_add_ps(v2, v3))));

	// R2
	v0 = _mm_mul_ps(_mm_load1_ps(&_31), rhs.v[0]);
	v1 = _mm_mul_ps(_mm_load1_ps(&_32), rhs.v[1]);
	v2 = _mm_mul_ps(_mm_load1_ps(&_33), rhs.v[2]);
	v3 = _mm_mul_ps(_mm_load1_ps(&_34), rhs.v[3]);
	__m128 r2(_mm_add_ps(v0, _mm_add_ps(v1, _mm_add_ps(v2, v3))));

	// R3
	v0 = _mm_mul_ps(_mm_load1_ps(&_41), rhs.v[0]);
	v1 = _mm_mul_ps(_mm_load1_ps(&_42), rhs.v[1]);
	v2 = _mm_mul_ps(_mm_load1_ps(&_43), rhs.v[2]);
	v3 = _mm_mul_ps(_mm_load1_ps(&_44), rhs.v[3]);
	__m128 r3(_mm_add_ps(v0, _mm_add_ps(v1, _mm_add_ps(v2, v3))));

	return Matrix_16f(r0, r1, r2, r3);
}

/**************************************************************************************************
* Calculate the determinant of the matrix
**************************************************************************************************/
float MatrixDeterminant_16f(const Matrix_16f* pM) {
	__m128 v0(_mm_mul_ps(pM->v[0], _mm_set_ps(-pM->_21, pM->_24, pM->_23, pM->_22)));
	__m128 v1(_mm_mul_ps(pM->v[1], _mm_set_ps(-pM->_11, pM->_14, pM->_13, pM->_12)));
	__m128 ans(_mm_sub_ps(v0, v1));
	
	v0 = _mm_mul_ps(_mm_set_ps(pM->_32, pM->_31, pM->_31, pM->_33), _mm_set_ps(pM->_43, pM->_42, pM->_44, pM->_44));
	v1 = _mm_mul_ps(_mm_set_ps(pM->_33, pM->_32, pM->_34, pM->_34), _mm_set_ps(pM->_42, pM->_41, pM->_41, pM->_43));
	v0 = _mm_sub_ps(v0, v1);
	ans = _mm_mul_ps(ans, v0);

	v0 = _mm_mul_ps(pM->v[0], _mm_set_ps(pM->_22, pM->_21, pM->_24, pM->_23));
	v1 = _mm_mul_ps(pM->v[2], _mm_set_ps(pM->_41, pM->_41, pM->_44, pM->_43));
	
	return ans.m128_f32[0] + ans.m128_f32[1] + ans.m128_f32[2] + ans.m128_f32[3] + 
		(v0.m128_f32[2] - v0.m128_f32[0]) * (v1.m128_f32[1] - v1.m128_f32[3]) + 
		(v0.m128_f32[3] - v0.m128_f32[1]) * (v1.m128_f32[0] - v1.m128_f32[2]);
}

/**************************************************************************************************
* Decompose the matrix into its constituent parts
**************************************************************************************************/
int MatrixDecompose_16f(Vector3_16f* pOutScale, Quaternion_16f* pOutRotation, Vector3_16f* pOutTranslation,
	const Matrix_16f* pM) {
	// Create row vectors u, v, and w
	Vector3_16f u(&pM->_11);
	Vector3_16f v(&pM->_21);
	Vector3_16f w(&pM->_31);

	// Generate three orthonormal vectors u*, v* and w* (we will simultaneously generate the scaling factors)
	float s1(Vec3Length_16f(&u));
	if(s1 == 0.0f)	return 0;
	Vector3_16f uO(u * (1.0f / s1));
	Vector3_16f temp(v - (Vec3Dot_16f(&v, &uO)*uO));
	float s2(Vec3Length_16f(&temp));
	if(s2 == 0.0f)	return 0;
	Vector3_16f vO(temp * (1.0f / s2));
	temp = (w - (Vec3Dot_16f(&v, &uO)*uO) - (Vec3Dot_16f(&w, &vO)*vO));
	float s3(Vec3Length_16f(&temp));
	if(s3 == 0.0f)	return 0;
	Vector3_16f wO(temp * (1.0f / s3));

	Matrix_16f R(
		uO.x, uO.y, uO.z, 0.0f,
		vO.x, vO.y, vO.z, 0.0f,
		wO.x, wO.y, wO.z, 0.0f,
		0.0f, 0.0f, 0.0f, 1.0f);
	float det(MatrixDeterminant_16f(&R));
	if(det == -1.0f) {
		R._31 *= -1.0f;
		R._32 *= -1.0f;
		R._33 *= -1.0f;
		s3 *= -1.0f;
	}

	// Populate scale
	pOutScale->x = s1;
	pOutScale->y = s2;
	pOutScale->z = s3;

	// Populate quaternion
	QuaternionRotationMatrix_16f(pOutRotation, &R);

	// Populate translation
	pOutTranslation->v = pM->v[3];
	
	return 1;
}

/**************************************************************************************************
* Transpose the matrix
**************************************************************************************************/
Matrix_16f* MatrixTranspose(Matrix_16f* pOut, const Matrix_16f* pM) {
	pOut->_11 = pM->_11;	pOut->_12 = pM->_21;	pOut->_13 = pM->_31;	pOut->_14 = pM->_41;
	pOut->_21 = pM->_12;	pOut->_22 = pM->_22;	pOut->_23 = pM->_32;	pOut->_24 = pM->_42;
	pOut->_31 = pM->_13;	pOut->_32 = pM->_23;	pOut->_33 = pM->_33;	pOut->_34 = pM->_43;
	pOut->_41 = pM->_14;	pOut->_42 = pM->_24;	pOut->_43 = pM->_34;	pOut->_44 = pM->_44;
	return pOut;
}

/**************************************************************************************************
* Multiply two matrices and return the result
**************************************************************************************************/
Matrix_16f* MatrixMultiply_16f(Matrix_16f* pOut, const Matrix_16f* pM1, const Matrix_16f* pM2) {
	// R0
	// Single vectors representing a particular row-column value
	// Create the resultant row values by multiplying across the matrix columns
	// (a*a, a*b, a*c, a*d), (b*e, b*f, b*g, b*h), ...
	// These will then be summed across to yield the row values
	__m128 v0(_mm_mul_ps(_mm_load1_ps(&pM1->_11), pM2->v[0]));
	__m128 v1(_mm_mul_ps(_mm_load1_ps(&pM1->_12), pM2->v[1]));
	__m128 v2(_mm_mul_ps(_mm_load1_ps(&pM1->_13), pM2->v[2]));
	__m128 v3(_mm_mul_ps(_mm_load1_ps(&pM1->_14), pM2->v[3]));

	// Sum respective column values
	pOut->v[0] = _mm_add_ps(v0, _mm_add_ps(v1, _mm_add_ps(v2, v3)));

	// R1
	v0 = _mm_mul_ps(_mm_load1_ps(&pM1->_21), pM2->v[0]);
	v1 = _mm_mul_ps(_mm_load1_ps(&pM1->_22), pM2->v[1]);
	v2 = _mm_mul_ps(_mm_load1_ps(&pM1->_23), pM2->v[2]);
	v3 = _mm_mul_ps(_mm_load1_ps(&pM1->_24), pM2->v[3]);
	pOut->v[1] = _mm_add_ps(v0, _mm_add_ps(v1, _mm_add_ps(v2, v3)));

	// R2
	v0 = _mm_mul_ps(_mm_load1_ps(&pM1->_31), pM2->v[0]);
	v1 = _mm_mul_ps(_mm_load1_ps(&pM1->_32), pM2->v[1]);
	v2 = _mm_mul_ps(_mm_load1_ps(&pM1->_33), pM2->v[2]);
	v3 = _mm_mul_ps(_mm_load1_ps(&pM1->_34), pM2->v[3]);
	pOut->v[2] = _mm_add_ps(v0, _mm_add_ps(v1, _mm_add_ps(v2, v3)));

	// R3
	v0 = _mm_mul_ps(_mm_load1_ps(&pM1->_41), pM2->v[0]);
	v1 = _mm_mul_ps(_mm_load1_ps(&pM1->_42), pM2->v[1]);
	v2 = _mm_mul_ps(_mm_load1_ps(&pM1->_43), pM2->v[2]);
	v3 = _mm_mul_ps(_mm_load1_ps(&pM1->_44), pM2->v[3]);
	pOut->v[3] = _mm_add_ps(v0, _mm_add_ps(v1, _mm_add_ps(v2, v3)));
	return pOut;
}

/**************************************************************************************************
* Multiply two matrices and then take the transpose
**************************************************************************************************/
Matrix_16f* MatrixMultiplyTranspose_16f(Matrix_16f* pOut, const Matrix_16f* pM1, const Matrix_16f* pM2) {
	Matrix_16f m;
	MatrixMultiply_16f(&m, pM1, pM2);
	MatrixTranspose(pOut, &m);
	return pOut;
}

/**************************************************************************************************
* Return the inverse of the matrix as well as the determinant if the answer is non-null
**************************************************************************************************/
Matrix_16f* MatrixInverse_16f(Matrix_16f* pOut, float* pDeterminant, const Matrix_16f* pM) {
	// First, calculate the determinant
	float determinant(MatrixDeterminant_16f(pM));

	// Only proceed if determinant is non-zero
	if(determinant != 0.0f) {
		// Set the inverse matrix to start as the identity matrix (acts as the right-hand portion of 
		// the augmented matrix)
		MatrixIdentity_16f(pOut);
		Matrix_16f base(*pM);

		// Iterate through all pivots
		for(int pivot(0); pivot < 4; ++pivot) {
			// If the current pivot is not one, we must divide the row
			if(base.m[pivot][pivot] != 1.0f) {
				// If divideBy number would be zero, we must first make this row properly divisible
				if(base.m[pivot][pivot] == 0.0f) {
					for(int ar = pivot + 1; ar < 4; ++ar) {
						if(base.m[ar][pivot] != 0.0f) {
							base.v[pivot] = _mm_add_ps(base.v[pivot], base.v[ar]);
							pOut->v[pivot] = _mm_add_ps(pOut->v[pivot], pOut->v[ar]);
							ar = 4;
						}
					}
				}

				// Divide the row
				__m128 divideBy(_mm_load1_ps(&base.m[pivot][pivot]));
				base.v[pivot] = _mm_div_ps(base.v[pivot], divideBy);
				pOut->v[pivot] = _mm_div_ps(pOut->v[pivot], divideBy);
			}

			// Perform necessary row operations to make the current diagonal the leading row value
			for(int r(0); r < 4; ++r) {
				if(r != pivot && base.m[r][pivot] != 0.0f) {
					// Subtract the current diagonal's row from this row such that the value is equal to zero
					__m128 times(_mm_load1_ps(&base.m[r][pivot]));
					base.v[r] = _mm_sub_ps(base.v[r], _mm_mul_ps(times, base.v[pivot]));
					pOut->v[r] = _mm_sub_ps(pOut->v[r], _mm_mul_ps(times, pOut->v[pivot]));
				}
			}
		}
	} else {
		pOut = 0;
	}

	if(pDeterminant) {
		*pDeterminant = determinant;
	}

	return pOut;
}

/**************************************************************************************************
* Build a matrix which scales by (sx, sy, sz)
**************************************************************************************************/
Matrix_16f* MatrixScaling_16f(Matrix_16f* pOut, float sx, float sy, float sz) {
	pOut->_11 = sx;		pOut->_12 = 0.0f;	pOut->_13 = 0.0f;	pOut->_14 = 0.0f;
	pOut->_21 = 0.0f;	pOut->_22 = sy;		pOut->_23 = 0.0f;	pOut->_24 = 0.0f;
	pOut->_31 = 0.0f;	pOut->_32 = 0.0f;	pOut->_33 = sz;		pOut->_34 = 0.0f;
	pOut->_41 = 0.0f;	pOut->_42 = 0.0f;	pOut->_43 = 0.0f;	pOut->_44 = 1.0f;
	return pOut;
}

/**************************************************************************************************
* Build a matrix which translates by (x, y, z)
**************************************************************************************************/
Matrix_16f* MatrixTranslation_16f(Matrix_16f* pOut, float x, float y, float z) {
	pOut->_11 = 0.0f;	pOut->_12 = 0.0f;	pOut->_13 = 0.0f;	pOut->_14 = 0.0f;
	pOut->_21 = 0.0f;	pOut->_22 = 0.0f;	pOut->_23 = 0.0f;	pOut->_24 = 0.0f;
	pOut->_31 = 0.0f;	pOut->_32 = 0.0f;	pOut->_33 = 0.0f;	pOut->_34 = 0.0f;
	pOut->_41 = x;		pOut->_42 = y;		pOut->_43 = z;		pOut->_44 = 0.0f;
	return pOut;
}

/**************************************************************************************************
* Build a matrix which rotates about the X axis
**************************************************************************************************/
Matrix_16f* MatrixRotationX_16f(Matrix_16f* pOut, float angle) {
	float s = sin(angle);
	float c = cos(angle);
	pOut->_11 = 1.0f;	pOut->_12 = 0.0f;	pOut->_13 = 0.0f;	pOut->_14 = 0.0f;
	pOut->_21 = 0.0f;	pOut->_22 = c;		pOut->_23 = s;		pOut->_24 = 0.0f;
	pOut->_31 = 0.0f;	pOut->_32 = -s;		pOut->_33 = c;		pOut->_34 = 0.0f;
	pOut->_41 = 0.0f;	pOut->_42 = 0.0f;	pOut->_43 = 0.0f;	pOut->_44 = 1.0f;
	return pOut;
}

/**************************************************************************************************
* Build a matrix which rotates about the Y axis
**************************************************************************************************/
Matrix_16f* MatrixRotationY_16f(Matrix_16f* pOut, float angle) {
	float s = sin(angle);
	float c = cos(angle);
	pOut->_11 = c;		pOut->_12 = 0.0f;	pOut->_13 = -s;		pOut->_14 = 0.0f;
	pOut->_21 = 0.0f;	pOut->_22 = 1.0f;	pOut->_23 = 0.0f;	pOut->_24 = 0.0f;
	pOut->_31 = s;		pOut->_32 = 0.0f;	pOut->_33 = c;		pOut->_34 = 0.0f;
	pOut->_41 = 0.0f;	pOut->_42 = 0.0f;	pOut->_43 = 0.0f;	pOut->_44 = 1.0f;
	return pOut;
}

/**************************************************************************************************
* Build a matrix which rotates about the Z axis
**************************************************************************************************/
Matrix_16f* MatrixRotationZ_16f(Matrix_16f* pOut, float angle) {
	float s = sin(angle);
	float c = cos(angle);
	pOut->_11 = c;		pOut->_12 = s;		pOut->_13 = 0.0f;	pOut->_14 = 0.0f;
	pOut->_21 = -s;		pOut->_22 = c;		pOut->_23 = 0.0f;	pOut->_24 = 0.0f;
	pOut->_31 = 0.0f;	pOut->_32 = 0.0f;	pOut->_33 = 0.0f;	pOut->_34 = 0.0f;
	pOut->_41 = 0.0f;	pOut->_42 = 0.0f;	pOut->_43 = 0.0f;	pOut->_44 = 1.0f;
	return pOut;
}

/**************************************************************************************************
* Build a matrix which rotates around an arbitrary axis
**************************************************************************************************/
Matrix_16f* MatrixRotationAxis_16f(Matrix_16f* pOut, const Vector3_16f* pV, float angle) {
	float s(sin(angle));
	float c(cos(angle));
	__m128 diffC(_mm_set1_ps(1.0f - c));
	pOut->_11 = (pV->x * pV->x);
	pOut->_12 = (pV->x * pV->y);
	pOut->_13 = (pV->x * pV->z);
	pOut->_14 = 0.0f;
	pOut->v[0] = _mm_mul_ps(pOut->v[0], diffC);
	pOut->_11 += c;
	pOut->_12 += pV->z * s;
	pOut->_13 -= pV->y * s;
	pOut->_21 = (pV->x * pV->y);
	pOut->_22 = (pV->y * pV->y);
	pOut->_23 = (pV->y * pV->z);
	pOut->_24 = 0.0f;
	pOut->v[1] = _mm_mul_ps(pOut->v[1], diffC);
	pOut->_21 -= pV->z * s;
	pOut->_22 += c;
	pOut->_23 += pV->x * s;
	pOut->_31 = (pV->x * pV->z);
	pOut->_32 = (pV->y * pV->z);
	pOut->_33 = (pV->z * pV->z);
	pOut->_34 = 0.0f;
	pOut->v[2] = _mm_mul_ps(pOut->v[2], diffC);
	pOut->_31 += pV->y * s;
	pOut->_32 -= pV->x * s;
	pOut->_33 += c;
	pOut->_41 = 0.0f;	pOut->_42 = 0.0f;	pOut->_43 = 0.0f;	pOut->_44 = 1.0f;
	return pOut;
}

/**************************************************************************************************
* Build a matrix from a quaternion
**************************************************************************************************/
Matrix_16f* MatrixRotationQuaternion_16f(Matrix_16f* pOut, const Quaternion_16f* pQ) {
	pOut->_11 = 1.0f - 2.0f*pQ->y*pQ->y - 2.0f*pQ->z*pQ->z;
	pOut->_12 = 2.0f*pQ->x*pQ->y - 2.0f*pQ->w*pQ->z;
	pOut->_13 = 2.0f*pQ->x*pQ->z + 2.0f*pQ->w*pQ->y;
	pOut->_14 = 0.0f;
	pOut->_21 = 2.0f*pQ->x*pQ->y + 2.0f*pQ->w*pQ->z;
	pOut->_22 = 1.0f - 2.0f*pQ->x*pQ->x - 2.0f*pQ->z*pQ->z;
	pOut->_23 = 2.0f*pQ->y*pQ->z - 2.0f*pQ->w*pQ->x;
	pOut->_24 = 0.0f;
	pOut->_31 = 2.0f*pQ->x*pQ->z - 2.0f*pQ->w*pQ->y;
	pOut->_32 = 2.0f*pQ->y*pQ->z + 2.0f*pQ->w*pQ->x;
	pOut->_33 = 1.0f - 2.0f*pQ->x*pQ->x - 2.0f*pQ->y*pQ->y;
	pOut->_34 = 0.0f;
	pOut->_41 = 0.0f;
	pOut->_42 = 0.0f;
	pOut->_43 = 0.0f;
	pOut->_44 = 1.0f;
	return pOut;
}


/**************************************************************************************************
* Yaw around the Y axis, a pitch around the X axis, and a roll around the Z axis
**************************************************************************************************/
Matrix_16f* MatrixRotationYawPitchRoll_16f(Matrix_16f* pOut, float yaw, float pitch, float roll) {
	Matrix_16f m1, m2, m3;

	// Get the yaw and pitch rotations
	MatrixRotationY_16f(&m1, yaw);
	MatrixRotationX_16f(&m2, pitch);
	MatrixMultiply_16f(&m3, &m1, &m2);

	// Find the roll
	MatrixRotationZ_16f(&m1, roll);

	// Find the cumulative rotation
	MatrixMultiply_16f(pOut, &m3, &m1);

	// Return the end result
	return pOut;
}

/**************************************************************************************************
* Build transformation matrix with null arguments being treated as identity. 
* Mout = Msc-1 * Msr-1 * Ms * Msr * Msc * Mrc-1 * Mr * Mrc * Mt
**************************************************************************************************/
Matrix_16f* MatrixTransformation_16f(Matrix_16f* pOut, const Vector3_16f* pScalingCenter, 
	const Quaternion_16f* pScalingRotation, const Vector3_16f* pScaling, const Vector3_16f* pRotationCenter, 
	const Quaternion_16f* pRotation, const Vector3_16f* pTranslation) {
	// Build the various matrices required
	Matrix_16f mSC;
	if(pScalingCenter)	MatrixTranslation_16f(&mSC, pScalingCenter->x, pScalingCenter->y, pScalingCenter->z);
	else				MatrixIdentity_16f(&mSC);

	Matrix_16f mSCI;	MatrixInverse_16f(&mSCI, 0, &mSC);

	Matrix_16f mSR;
	if(pScalingRotation)	MatrixRotationQuaternion_16f(&mSR, pScalingRotation);
	else					MatrixIdentity_16f(&mSR);

	Matrix_16f mSRI;	MatrixInverse_16f(&mSRI, 0, &mSR);

	Matrix_16f mS;
	if(pScaling)	MatrixScaling_16f(&mS, pScaling->x, pScaling->y, pScaling->z);
	else			MatrixIdentity_16f(&mS);

	Matrix_16f mRC;
	if(pRotationCenter)	MatrixTranslation_16f(&mRC, pRotationCenter->x, pRotationCenter->y, pRotationCenter->z);
	else				MatrixIdentity_16f(&mRC);

	Matrix_16f mRCI;	MatrixInverse_16f(&mRCI, 0, &mRC);

	Matrix_16f mR;
	if(pRotation)	MatrixRotationQuaternion_16f(&mR, pRotation);
	else			MatrixIdentity_16f(&mR);

	Matrix_16f mT;
	if(pTranslation)	MatrixTranslation_16f(&mT, pTranslation->x, pTranslation->y, pTranslation->z);
	else				MatrixIdentity_16f(&mT);

	Matrix_16f m1, m2;
	MatrixMultiply_16f(&m1, &mSCI, &mSRI);
	MatrixMultiply_16f(&m2, &m1, &mS);
	MatrixMultiply_16f(&m1, &m2, &mSR);
	MatrixMultiply_16f(&m2, &m1, &mSC);
	MatrixMultiply_16f(&m1, &m2, &mRCI);
	MatrixMultiply_16f(&m2, &m1, &mR);
	MatrixMultiply_16f(&m1, &m2, &mRC);
	MatrixMultiply_16f(pOut, &m1, &mT);
	return pOut;
}


/**************************************************************************************************
* Build a 2D transformation matrix in XY plane. Null arguments treated as identity.
* Mout = Msc-1 * Msr-1 * Ms * Msr * Msc * Mrc-1 * Mr * Mrc * Mt
**************************************************************************************************/
Matrix_16f* MatrixTransformation2D_16f(Matrix_16f* pOut, const Vector2_16f* pScalingCenter,
	float scalingRotation, const Vector2_16f* pScaling, const Vector2_16f* pRotationCenter, 
	float rotation, const Vector2_16f* pTranslation) {
	// Build the various matrices
	Matrix_16f mSC;
	if(pScalingCenter)	MatrixTranslation_16f(&mSC, pScalingCenter->x, pScalingCenter->y, 0.0f);
	else				MatrixIdentity_16f(&mSC);

	Matrix_16f mSCI;
	MatrixInverse_16f(&mSCI, 0, &mSC);

	Matrix_16f mSR;		MatrixRotationYawPitchRoll_16f(&mSR, scalingRotation, scalingRotation, 0.0f);

	Matrix_16f mSRI;	MatrixInverse_16f(&mSRI, 0, &mSR);

	Matrix_16f mS;
	if(pScaling)		MatrixScaling_16f(&mS, pScaling->x, pScaling->y, 0.0f);
	else				MatrixIdentity_16f(&mS);

	Matrix_16f mRC;
	if(pRotationCenter)	MatrixTranslation_16f(&mRC, pRotationCenter->x, pRotationCenter->y, 0.0f);
	else				MatrixIdentity_16f(&mRC);

	Matrix_16f mRCI;	MatrixInverse_16f(&mRCI, 0, &mRC);

	Matrix_16f mR;		MatrixRotationYawPitchRoll_16f(&mR, rotation, rotation, 0.0f);

	Matrix_16f mT;
	if(pTranslation)	MatrixTranslation_16f(&mT, pTranslation->x, pTranslation->y, 0.0f);
	else				MatrixIdentity_16f(&mT);

	Matrix_16f m1, m2;
	MatrixMultiply_16f(&m1, &mSCI, &mSRI);
	MatrixMultiply_16f(&m2, &m1, &mS);
	MatrixMultiply_16f(&m1, &m2, &mSR);
	MatrixMultiply_16f(&m2, &m1, &mSC);
	MatrixMultiply_16f(&m1, &m2, &mRCI);
	MatrixMultiply_16f(&m2, &m1, &mR);
	MatrixMultiply_16f(&m1, &m2, &mRC);
	MatrixMultiply_16f(pOut, &m1, &mT);
	return pOut;
}

/**************************************************************************************************
* Build affine transformation matrix. Null arguments treated as identity. 
* Mout = Ms * Mrc-1 * Mr * Mrc * Mt
**************************************************************************************************/
Matrix_16f* MatrixAffineTransformation_16f(Matrix_16f* pOut, float scaling, const Vector3_16f* pRotationCenter, 
	const Quaternion_16f* pRotation, const Vector3_16f* pTranslation) {
	// Build various matrices
	Matrix_16f mS;
	MatrixScaling_16f(&mS, scaling, scaling, scaling);

	Matrix_16f mRC;
	if(pRotationCenter)	MatrixTranslation_16f(&mRC, pRotationCenter->x, pRotationCenter->y, pRotationCenter->z);
	else				MatrixIdentity_16f(&mRC);

	Matrix_16f mRCI;
	MatrixInverse_16f(&mRCI, 0, &mRC);

	Matrix_16f mR;
	if(pRotation)	MatrixRotationQuaternion_16f(&mR, pRotation);
	else			MatrixIdentity_16f(&mR);

	Matrix_16f mT;
	if(pTranslation)	MatrixTranslation_16f(&mT, pTranslation->x, pTranslation->y, pTranslation->z);
	else				MatrixIdentity_16f(&mT);

	Matrix_16f m1, m2;
	MatrixMultiply_16f(&m1, &mS, &mRCI);
	MatrixMultiply_16f(&m2, &m1, &mR);
	MatrixMultiply_16f(&m1, &m2, &mRC);
	MatrixMultiply_16f(pOut, &m1, &mT);
	return pOut;
}


/**************************************************************************************************
* Build 2D affine transformation matrix in XY plane. Null arguments treated as identity. 
* Mout = Ms * Mrc-1 * Mr * Mrc * Mt
**************************************************************************************************/
Matrix_16f* MatrixAffineTransformation2D_16f(Matrix_16f* pOut, float scaling, const Vector2_16f* pRotationCenter, 
	float rotation, const Vector2_16f* pTranslation) {
	// Build the various matrices
	Matrix_16f mS;	MatrixScaling_16f(&mS, scaling, scaling, 0.0f);
	
	Matrix_16f mRC;		
	if(pRotationCenter)	MatrixTranslation_16f(&mRC, pRotationCenter->x, pRotationCenter->y, 0.0f);
	else				MatrixIdentity_16f(&mRC);
	
	Matrix_16f mRCI;	MatrixInverse_16f(&mRCI, 0, &mRC);
	
	Matrix_16f mR;		MatrixRotationYawPitchRoll_16f(&mR, rotation, rotation, 0.0f);
	
	Matrix_16f mT;		
	if(pTranslation)	MatrixTranslation_16f(&mT, pTranslation->x, pTranslation->y, 0.0f);
	else				MatrixIdentity_16f(&mT);
	
	Matrix_16f m1, m2;
	MatrixMultiply_16f(&m1, &mS, &mRCI);
	MatrixMultiply_16f(&m2, &m1, &mR);
	MatrixMultiply_16f(&m1, &m2, &mRC);
	MatrixMultiply_16f(pOut, &m1, &mT);
	return pOut;
}

/**************************************************************************************************
* Build a look at matrix (right handed)
**************************************************************************************************/
Matrix_16f* MatrixLookAtRH_16f(Matrix_16f* pOut, const Vector3_16f* pEye, const Vector3_16f* pAt, 
	const Vector3_16f* pUp) {
	// Construct each local axis
	Vector3_16f zAxis;	Vec3Normalize_16f(&zAxis, Vec3Sub_16f(&zAxis, pEye, pAt));
	Vector3_16f xAxis;	Vec3Normalize_16f(&xAxis, Vec3Cross_16f(&xAxis, pUp, &zAxis));
	Vector3_16f yAxis;	Vec3Cross_16f(&yAxis, &zAxis, &xAxis);

	pOut->_11 = xAxis.x;	pOut->_12 = yAxis.x;	pOut->_13 = zAxis.x;	pOut->_14 = 0.0f;
	pOut->_21 = xAxis.y;	pOut->_22 = yAxis.y;	pOut->_23 = zAxis.y;	pOut->_24 = 0.0f;
	pOut->_31 = xAxis.z;	pOut->_32 = yAxis.z;	pOut->_33 = zAxis.z;	pOut->_34 = 0.0f;
	pOut->_41 = Vec3Dot_16f(&xAxis, pEye);
	pOut->_42 = Vec3Dot_16f(&yAxis, pEye);
	pOut->_43 = Vec3Dot_16f(&zAxis, pEye);
	pOut->_44 = 1.0f;
	return pOut;
}

/**************************************************************************************************
* Build a look at matrix (left-handed)
**************************************************************************************************/
Matrix_16f* MatrixLookAtLH_16f(Matrix_16f* pOut, const Vector3_16f* pEye, const Vector3_16f* pAt, 
	const Vector3_16f* pUp) {
	// Construct each local axis
	Vector3_16f zAxis;	Vec3Normalize_16f(&zAxis, Vec3Sub_16f(&zAxis, pAt, pEye));
	Vector3_16f xAxis;	Vec3Normalize_16f(&xAxis, Vec3Cross_16f(&xAxis, pUp, &zAxis));
	Vector3_16f yAxis;	Vec3Cross_16f(&yAxis, &zAxis, &xAxis);

	pOut->_11 = xAxis.x;	pOut->_12 = yAxis.x;	pOut->_13 = zAxis.x;	pOut->_14 = 0.0f;
	pOut->_21 = xAxis.y;	pOut->_22 = yAxis.y;	pOut->_23 = zAxis.y;	pOut->_24 = 0.0f;
	pOut->_31 = xAxis.z;	pOut->_32 = yAxis.z;	pOut->_33 = zAxis.z;	pOut->_34 = 0.0f;
	pOut->_41 = -Vec3Dot_16f(&xAxis, pEye);
	pOut->_42 = -Vec3Dot_16f(&yAxis, pEye);
	pOut->_43 = -Vec3Dot_16f(&zAxis, pEye);
	pOut->_44 = 1.0f;
	return pOut; 
}

/**************************************************************************************************
* Build a perspective projection matrix (right-handed)
**************************************************************************************************/
Matrix_16f* MatrixPerspectiveRH_16f(Matrix_16f* pOut, float w, float h, float zn, float zf) {
	pOut->_11 = 2.0f * zn / w;
	pOut->_12 = 0.0f;
	pOut->_13 = 0.0f;
	pOut->_14 = 0.0f;
	pOut->_21 = 0.0f;
	pOut->_22 = 2.0f * zn / h;
	pOut->_23 = 0.0f;
	pOut->_24 = 0.0f;
	pOut->_31 = 0.0f;
	pOut->_32 = 0.0f;
	pOut->_33 = zf / (zn - zf);
	pOut->_34 = -1.0f;
	pOut->_41 = 0.0f;
	pOut->_42 = 0.0f;
	pOut->_43 = (zn * zf) / (zn - zf);
	pOut->_44 = 0.0f;
	return pOut;
}

/**************************************************************************************************
* Build a perspective projection matrix (left-handed)
**************************************************************************************************/
Matrix_16f* MatrixPerspectiveLH_16f(Matrix_16f* pOut, float w, float h, float zn, float zf) {
	pOut->_11 = 2.0f * zn / w;
	pOut->_12 = 0.0f;
	pOut->_13 = 0.0f;
	pOut->_14 = 0.0f;
	pOut->_21 = 0.0f;
	pOut->_22 = 2.0f * zn / h;
	pOut->_23 = 0.0f;
	pOut->_24 = 0.0f;
	pOut->_31 = 0.0f;
	pOut->_32 = 0.0f;
	pOut->_33 = zf / (zf - zn);
	pOut->_34 = 1.0f;
	pOut->_41 = 0.0f;
	pOut->_42 = 0.0f;
	pOut->_43 = (zn * zf) / (zn - zf);
	pOut->_44 = 0.0f;
	return pOut;
}

/**************************************************************************************************
* Build a perspective projection matrix (right-handed)
**************************************************************************************************/
Matrix_16f* MatrixPerspectiveFovRH_16f(Matrix_16f* pOut, float fovy, float aspect, float zn, float zf) {
	float yScale = atan(fovy * 0.5f);
	float xScale = yScale / aspect;

	pOut->_11 = xScale;
	pOut->_12 = 0.0f;
	pOut->_13 = 0.0f;
	pOut->_14 = 0.0f;
	pOut->_21 = 0.0f;
	pOut->_22 = yScale;
	pOut->_23 = 0.0f;
	pOut->_24 = 0.0f;
	pOut->_31 = 0.0f;
	pOut->_32 = 0.0f;
	pOut->_33 = zf / (zn - zf);
	pOut->_34 = -1.0f;
	pOut->_41 = 0.0f;
	pOut->_42 = 0.0f;
	pOut->_43 = (zn * zf) / (zn - zf);
	pOut->_44 = 0.0f;
	return pOut;
}

/**************************************************************************************************
* Build a perspective projection matrix (left-handed)
**************************************************************************************************/
Matrix_16f* MatrixPerspectiveFovLH_16f(Matrix_16f* pOut, float fovy, float aspect, float zn, float zf) {
	float yScale = atan(fovy * 0.5f);
	float xScale = yScale / aspect;

	pOut->_11 = xScale;
	pOut->_12 = 0.0f;
	pOut->_13 = 0.0f;
	pOut->_14 = 0.0f;
	pOut->_21 = 0.0f;
	pOut->_22 = yScale;
	pOut->_23 = 0.0f;
	pOut->_24 = 0.0f;
	pOut->_31 = 0.0f;
	pOut->_32 = 0.0f;
	pOut->_33 = zf / (zf - zn);
	pOut->_34 = 1.0f;
	pOut->_41 = 0.0f;
	pOut->_42 = 0.0f;
	pOut->_43 = -(zn * zf) / (zf - zn);
	pOut->_44 = 0.0f;
	return pOut;
}

/**************************************************************************************************
* Build a matrix which flattens geometry into a plane, as if casting a shadow from a light
**************************************************************************************************/
Matrix_16f* MatrixShadow_16f(Matrix_16f* pOut, const Vector4_16f* pLight, const Plane_16f* pPlane) {
	// Normalize the plane
	Plane_16f p;
	PlaneNormalize_16f(&p, pPlane);

	// Calculate the dot product between the plane and the vector
	__m128 m(_mm_mul_ps(pLight->v, pPlane->v));
	float d(-(m.m128_f32[0] + m.m128_f32[1] + m.m128_f32[2] + m.m128_f32[3]));

	// Construct the matrix
	pOut->v[0] = _mm_mul_ps(_mm_load1_ps(&p.a), pLight->v);
	pOut->v[1] = _mm_mul_ps(_mm_load1_ps(&p.b), pLight->v);
	pOut->v[2] = _mm_mul_ps(_mm_load1_ps(&p.c), pLight->v);
	pOut->v[3] = _mm_mul_ps(_mm_load1_ps(&p.d), pLight->v);
	pOut->_11 += d;
	pOut->_21 += d;
	pOut->_31 += d;
	pOut->_41 += d;

	return pOut;
}

/**************************************************************************************************
* Build a matrix which reflects the coordinate system about a plane
**************************************************************************************************/
Matrix_16f* MatrixReflect_16f(Matrix_16f* pOut, const Plane_16f* pPlane) {
	// Normalize the plane
	Plane_16f p;
	PlaneNormalize_16f(&p, pPlane);

	// Construct the matrix
	__m128 m(_mm_mul_ps(_mm_set1_ps(-2.0f), p.v));
	pOut->v[0] = _mm_mul_ps(m, _mm_load1_ps(&p.a));
	pOut->v[1] = _mm_mul_ps(m, _mm_load1_ps(&p.b));
	pOut->v[2] = _mm_mul_ps(m, _mm_load1_ps(&p.c));
	pOut->v[3] = _mm_mul_ps(m, _mm_load1_ps(&p.d));
	pOut->_11 += 1.0f;
	pOut->_14 = 0.0f;
	pOut->_22 += 1.0f;
	pOut->_24 = 0.0f;
	pOut->_33 += 1.0f;
	pOut->_34 = 0.0f;
	pOut->_44 = 1.0f;

	return pOut;
}
